#include "ball.h"

GLuint Ball::index = -100;
int Ball::numBallsLeft = 0;
int Ball::angle = 0;

Ball::Ball()
{
	position = Vector3();
	velocity = Vector3();
	ballHit = false;
	aboveFloor = true;
}

Ball::Ball(Vector3 pos)
{
	position = pos;
	velocity = Vector3();
	ballHit = false;
	aboveFloor = true;
}

Ball::Ball(Ball * b){
	position = b->position;
	velocity = b->velocity;
	timeTillNotHit = b->timeTillNotHit;
	aboveFloor = true;
}

void Ball::renderTimer()
{
	/*std::stringstream s;
	s << timeTillNotHit;
	string timeLeft = s.str();*/
	string timeLeft = intToString(timeTillNotHit);

	// Need to create a vector from the camera to the ball
	Vector3 cameraToBall = Vector3(this->getPosition().x - camera.x, 0, this->getPosition().z - camera.z);
	Vector3 y = Vector3(0, 1, 0);

	// Cross product that vector with (0, 1, 0) & normalize the result
	Vector3 rotationVec = (cameraToBall.crossProduct(y)).normalize();

	// Dot product rotationVec with (1, 0, 0) to get angle to rotate by
	Vector3 x = Vector3(1, 0, 0);
	double rotateBy = rotationVec.getAngle(x) * 180 / PI;
	
	// Figure out if positive or negative rotation
	double posOrNeg = cameraToBall.getAngle(x) * 180 / PI;
	if (posOrNeg < 90) rotateBy *= -1;

	glPushMatrix();
		// Move text above ball
		glTranslated(0, 60, 0);

		// Face camera
		glRotated(rotateBy, 0, 1, 0);

		// Adjust text for number of digits left in timer
		if (timeLeft.size() == 1)
		{
			glTranslated(-20, 0, 0);
		}
		else if (timeLeft.size() == 2)
		{
			glTranslated(-20, 0, 0);
		}
		else
		{
			glTranslated(-20, 0, 0);
		}
		
		freetype::print(our_font, 0, 0, timeLeft.c_str());
	glPopMatrix();
}



bool Ball::decTimer(){
	this->timeTillNotHit--;
	if(timeTillNotHit == 0){
		ballHit = false;
		numBallsLeft++;
		return false;
	}
	return true;
}

void Ball::draw(){
	float colorW[] = { 0.9f, 0.3f, 0.2f, 1.0f };

	glPushMatrix();
	glTranslateVector(position);
	if (this->velocity.magnitude() != 0)
	{
		Vector3 perpVec = this->velocity.crossProduct(Vector3(0,1,0)).normalize();
		
		/*glDisable(GL_LIGHTING);
		glColor4d(0, 0, 1, 1);
		glLineWidth(2.0f);
		glBegin(GL_LINES);
			glVertex3d(perpVec.x, 0, 0);
			glVertex3d(0, 0, perpVec.z);
		glEnd();
		glEnable(GL_LIGHTING);*/

		this->rollAngle = ((int)this->rollAngle + 1) % 360;
		glRotated(-this->rollAngle, perpVec.x, perpVec.y, perpVec.z);
	}
	glColor3fv(colorW);
	glCallList(index);
	//if (ballHit) renderTimer();
	glPopMatrix();
}

void Ball::DrawWarpZone()
{
	float color[] = { 1.0f, 0.0f, 1.0f, 1.0f };

	glPushMatrix();
		glTranslateVector(position);
		glRotated(angle, 0, 1, 0);
		glCallList(index + DRAW_WARP_ZONE);
	glPopMatrix();
}

void Ball::setupDisplayList(){
	index = glGenLists(NUM_DISPLAY_LISTS);

	glNewList(index, GL_COMPILE);
	glutSolidSphere(50, 40, 40);
	glEndList();

	glNewList(index + DRAW_WARP_ZONE, GL_COMPILE);
		glPushMatrix();
			glTranslated(BALL_RADIUS, 0, 0);
			glRotated(90, 1, 0, 0);
			glutSolidCylinder(5, 500, 30, 30);
		glPopMatrix();
		glPushMatrix();
			glTranslated(-BALL_RADIUS, 0, 0);
			glRotated(90, 1, 0, 0);
			glutSolidCylinder(5, 500, 30, 30);
		glPopMatrix();
		glPushMatrix();
			glTranslated(0, 0, BALL_RADIUS);
			glRotated(90, 1, 0, 0);
			glutSolidCylinder(5, 500, 30, 30);
		glPopMatrix();
		glPushMatrix();
			glTranslated(0, 0, -BALL_RADIUS);
			glRotated(90, 1, 0, 0);
			glutSolidCylinder(5, 500, 30, 30);
		glPopMatrix();
	glEndList();
}

void Ball::lightBall( float * color )
{
	GLfloat	specular[4]	= { 0.3f , 0.3f , 0.3f , 1.0f };

	glMaterialf(GL_FRONT , GL_SHININESS, 100.0f);
	glMaterialfv(GL_FRONT , GL_AMBIENT_AND_DIFFUSE, color);
	glMaterialfv(GL_FRONT , GL_SPECULAR, specular);
}

void Ball::setPosition( const Vector3 & pos )
{
	position = pos;
}

Vector3 Ball::getPosition()
{
	return position;
}

void Ball::setVelocity( const Vector3 & vel )
{
	velocity = vel;
}

Vector3 Ball::getVelocity()
{
	return velocity;
}

void Ball::updatePosition( double elapsedTime )
{
	position = position + velocity * (float) elapsedTime;
}

Vector3 Ball::getNextPosition(double elapsedTime){
	return position + velocity * (float) elapsedTime;
}

bool Ball::resolveCollision(Ball ball, float & closeCollision, Vector3 &nextVelocity, Vector3 &newNextPosition, Vector3 &newCurPos, Vector3 nextPos, double &maxTime, double timeFromStart){
	if((this->aboveFloor != ball.aboveFloor) && Hole_Is_Active) return false;
	Vector3 dis = this->getPosition() - ball.getPosition();
	Vector3 Vp, Vn, Vp2, Vn2;
	float magTestCurr, magTestNext, magNextTo;
	float a, b, c, ans1, ans2, timeOfCollision;

	a = (this->getVelocity() - ball.getVelocity()).dotProduct(this->getVelocity() - ball.getVelocity());
	b = ((this->getPosition() - ball.getPosition()) * 2).dotProduct(this->getVelocity() - ball.getVelocity());
	c = ((this->getPosition() - ball.getPosition()).dotProduct(this->getPosition() - ball.getPosition())) - (pow((BALL_RADIUS * 2.0f), 2));
	
	//make sure this returns false

		if(!quadraticEq(a, b, c, ans1, ans2)){
			//cout << a << ", " << b << ", " << c << endl;
			if     (ans1 > 0.0f && ans2 < 0.0f) timeOfCollision = ans1;
			else if(ans2 > 0.0f && ans1 < 0.0f) timeOfCollision = ans2;
			else ans1 < ans2 ? timeOfCollision = ans1 : timeOfCollision = ans2;
			//int f = RangeRand(0,400);
			//if(f < 3)
			//cout << timeOfCollision << endl;
			//cout << "Time1: " << ans1 << endl;
			//cout << "Time2: " << ans2 << endl;
			//cout << "Choice: " << timeOfCollision << endl;
			//Compare magnitudes here.  One with picked time and other with actual next position
			if(timeOfCollision <= maxTime && timeOfCollision > .00001f){
				timeFromStart -= timeOfCollision;
				/*magTestCurr = (ball.getPosition() - this->getPosition()).magnitude();
				magTestNext = (ball.getPosition() - nextPos).magnitude();*/
				if(timeOfCollision < closeCollision || closeCollision == -1){

					Vector3 tmp = this->position + this->velocity * timeOfCollision;

					//if ((tmp - ball.getPosition()).magnitude() <= 99.0f || (tmp - ball.getPosition()).magnitude() >= 101.0f) 
					//{
					//	cout << "failure" << endl;
					//}

					//float wiggleRoom = 3.0f;
					//tmp = tmp.normalize() * (tmp.magnitude());
					newCurPos = tmp;
					/*magNextTo = (ball.getPosition() - newCurPos).magnitude();*/

					closeCollision = timeOfCollision;

					if(billardsOn){
						//Vp = this->getVelocity().projectAtoB(ball.getPosition() - newCurPos);
						//Vn = this->getVelocity() - Vp;

						//Vp2 = ball.getVelocity().projectAtoB(newCurPos - ball.getPosition());
						//Vn2 = ball.getVelocity() - Vp2;

						//nextVelocity = (Vn2 + Vp);

						Vp = this->getVelocity().projectAtoB(ball.getPosition() - newCurPos);
						Vn = this->getVelocity() - Vp;
						nextVelocity = Vn - Vp;
					}
					else{
						Vp = this->getVelocity().projectAtoB(ball.getPosition() - newCurPos);
						Vn = this->getVelocity() - Vp;
						nextVelocity = Vn - Vp;
					}

					newNextPosition = newCurPos + (nextVelocity.normalize() * ((this->getPosition() - nextPos).magnitude() - (this->getPosition() - newCurPos).magnitude()));

					//if((newNextPosition - ball.getPosition()).magnitude() <= 100)
					//{
					//	cout << "ans1: " << ans1 << endl;
					//	cout << "ans2: " << ans2 << endl;
					//	cout << "timeOfCollision: " << timeOfCollision << endl;
					//	cout << "newNextPosition - ball: " << (newNextPosition - ball.getPosition()).magnitude() << endl;
  			//			newNextPosition = ball.getPosition() + (nextVelocity.normalize() * 100.1);
					//	cout << "newNextPosition - ball: " << (newNextPosition - ball.getPosition()).magnitude() << endl;
					//}

					//if((ball.getPosition() - newNextPosition).magnitude() < 100){
					//	cout << (this->getPosition() - nextPos).magnitude() - (this->getPosition() - newCurPos).magnitude() << endl;
					//	Vector3::printVectors("Vector added to newCurPos: %v", (nextVelocity.normalize() * ((this->getPosition() - nextPos).magnitude() - (this->getPosition() - newCurPos).magnitude())));
					//	cout << (nextVelocity.normalize() * ((this->getPosition() - nextPos).magnitude() - (this->getPosition() - newCurPos).magnitude())).magnitude() << endl;
					//	//newNextPosition = newNextPosition + (nextVelocity.normalize());
					//	cout << (ball.getPosition() - newCurPos).magnitude() << endl;
					//	cout << (ball.getPosition() - newNextPosition).magnitude() << endl;
					//	Vector3::printVectors("NewVelocity: %v", nextVelocity.normalize());
					//	Vector3::printVectors("directionVec: %v", (ball.getPosition() - newNextPosition).normalize());
					//}

					//Vector3::printVectors("Current Position %v\n",          this->getPosition());
					//Vector3::printVectors("Next Position %v\n",             nextPos);
					//Vector3::printVectors("Next to Ball Position %v\n",     newCurPos);
					//Vector3::printVectors("Reflected Position %v\n",        newNextPosition);
					//cout << "Magnitude from ball to new position: " << (ball.getPosition() - newNextPosition).magnitude() << endl;

					//Vector3::printVectors("Current Velocity %v\n",          this->getVelocity());
					//Vector3::printVectors("Next Velocity %v\n",             nextVelocity);
					maxTime = timeFromStart;
					return true;
				}
			}
		}

	return false;
}

void Ball::startTimer(){
	if(!ballHit)
		numBallsLeft--;
	ballHit = true;
	timeTillNotHit = startingTime;
}